{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from control.matlab import *\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "zc = 0.22\n",
    "dt = 0.01\n",
    "t_preview = 1\n",
    "Qe = 1e-4\n",
    "R = 1e-6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_preview_control_parameter(zc, dt, t_preview, Qe, R):\n",
    "    g = 9.81\n",
    "    A = np.matrix([[0, 1, 0],\n",
    "                   [0, 0, 1],\n",
    "                   [0, 0, 0]])\n",
    "    B = np.matrix([[0],\n",
    "                   [0],\n",
    "                   [1]])\n",
    "    C = np.matrix([[1, 0, -zc/g]])\n",
    "    D = 0;\n",
    "    \n",
    "    sys_c = ss(A, B, C, D);            \n",
    "    sys_d = c2d(sys_c, dt);\n",
    "    [A_d, B_d, C_d, D_d] = ssdata(sys_d);\n",
    "\n",
    "    C_d_dot_A_d = C_d*A_d\n",
    "\n",
    "    C_d_dot_B_d = C_d*B_d\n",
    "\n",
    "    A_tilde = np.matrix([[1, C_d_dot_A_d[0,0], C_d_dot_A_d[0,1], C_d_dot_A_d[0,2]],\n",
    "                         [0, A_d[0,0], A_d[0,1], A_d[0,2]],\n",
    "                         [0, A_d[1,0], A_d[1,1], A_d[1,2]],\n",
    "                         [0, A_d[2,0], A_d[2,1], A_d[2,2]]])\n",
    "    B_tilde = np.matrix([[C_d_dot_B_d[0,0]],\n",
    "                         [B_d[0,0]],\n",
    "                         [B_d[1,0]],\n",
    "                         [B_d[2,0]]])\n",
    "    C_tilde = np.matrix([[1, 0, 0, 0]])\n",
    "\n",
    "    Q = np.matrix([[Qe, 0, 0, 0],\n",
    "                   [0, 0, 0, 0],\n",
    "                   [0, 0, 0, 0],\n",
    "                   [0, 0, 0, 0]])\n",
    "\n",
    "    P, _, K  = dare(A_tilde, B_tilde, Q, R);\n",
    "\n",
    "    Gi = K[0,0:1]\n",
    "    Gx = K[0,1:]\n",
    "\n",
    "    N = np.arange(0,t_preview,dt).reshape(1,-1)\n",
    "\n",
    "    Gd = np.zeros(N.shape)\n",
    "\n",
    "    Gd[0,0] = -Gi\n",
    "\n",
    "    Ac_tilde = A_tilde - B_tilde * K;\n",
    "\n",
    "    I_tilde = np.matrix([[1],[0],[0],[0]])\n",
    "\n",
    "    X_tilde = -Ac_tilde.T*P*I_tilde;\n",
    "\n",
    "    for i in range(1, N.shape[1]):\n",
    "        Gd[0,i] = (R+B_tilde.T*P*B_tilde)**(-1)*B_tilde.T*X_tilde;\n",
    "        X_tilde = Ac_tilde.T*X_tilde;\n",
    "        \n",
    "    return np.array(A_d), np.array(B_d), np.array(C_d), np.array(Gi), np.array(Gx), Gd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "A_d, B_d, C_d, Gi, Gx, Gd = get_preview_control_parameter(zc, dt, t_preview, Qe, R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ -9.23868228,  -9.25780946,  -9.29295039,  -9.34116283,\n",
       "         -9.39967715,  -9.46589683,  -9.53739812,  -9.61192901,\n",
       "         -9.68740739,  -9.76191866,  -9.83371265,  -9.90120012,\n",
       "         -9.96294866, -10.01767828, -10.06425652, -10.1016933 ,\n",
       "        -10.12913544, -10.14586098, -10.15127328, -10.14489495,\n",
       "        -10.12636167, -10.09541596, -10.0519008 ,  -9.99575338,\n",
       "         -9.92699868,  -9.84574324,  -9.75216889,  -9.64652663,\n",
       "         -9.52913056,  -9.40035193,  -9.26061344,  -9.11038352,\n",
       "         -8.95017094,  -8.78051952,  -8.60200302,  -8.41522032,\n",
       "         -8.22079072,  -8.0193495 ,  -7.81154367,  -7.59802802,\n",
       "         -7.37946131,  -7.15650272,  -6.92980857,  -6.70002919,\n",
       "         -6.46780611,  -6.23376938,  -5.9985352 ,  -5.76270369,\n",
       "         -5.52685694,  -5.29155722,  -5.0573454 ,  -4.82473961,\n",
       "         -4.594234  ,  -4.3662978 ,  -4.14137443,  -3.91988089,\n",
       "         -3.70220721,  -3.48871616,  -3.27974297,  -3.07559531,\n",
       "         -2.87655334,  -2.68286986,  -2.49477062,  -2.31245468,\n",
       "         -2.13609491,  -1.96583856,  -1.80180789,  -1.64410092,\n",
       "         -1.49279214,  -1.34793344,  -1.20955492,  -1.07766585,\n",
       "         -0.95225564,  -0.83329482,  -0.72073605,  -0.61451518,\n",
       "         -0.51455228,  -0.42075269,  -0.3330081 ,  -0.25119755,\n",
       "         -0.17518857,  -0.10483813,  -0.03999369,   0.01950576,\n",
       "          0.07382877,   0.12315043,   0.16765144,   0.20751724,\n",
       "          0.24293707,   0.2741032 ,   0.30121005,   0.32445348,\n",
       "          0.34403001,   0.36013613,   0.37296767,   0.3827191 ,\n",
       "          0.38958303,   0.39374961,   0.39540606,   0.39473617]])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Gd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
